import gc
import time
from machine import *
from smartcar import *
from seekfree import *
from display import *
import math
import utime
from array import *
import my_ccd
from my_imu import *
from speed_control import SpeedManager, SpeedParams
from kalman_imu import *
from my_menu import *
from kalman_filter_imu import *
# 编码器滤波参数
EMA_ALPHA = 0.3  # 指数移动平均的平滑因子
MA_WINDOW_SIZE = 5  # 移动平均窗口大小

# 编码器数据缓存
speed_l_history = [0] * MA_WINDOW_SIZE
speed_r_history = [0] * MA_WINDOW_SIZE
speed_l_ema = 0
speed_r_ema = 0
history_idx = 0

kfp_var_l = {  
    'P': 1,      # 估算方差
    'G': 0.0,    # 卡尔曼增益
    'Q': 1.0,  # 过程噪声,Q增大,动态响应变快,收敛稳定性变坏
    'R': 3.0,   # 测量噪声,R增大,动态响应变慢,收敛稳定性变好
    'Output': 0  # 卡尔曼滤波器输出
}
kfp_var_r = {  
    'P': 1,      # 估算方差
    'G': 0.0,    # 卡尔曼增益
    'Q': 1.0,  # 过程噪声,Q增大,动态响应变快,收敛稳定性变坏
    'R': 3.0,   # 测量噪声,R增大,动态响应变慢,收敛稳定性变好
    'Output': 0  # 卡尔曼滤波器输出
}
def kalman_filter(kfp, input):
    # 估算方差方程
    kfp['P'] = kfp['P'] + kfp['Q']


    # 卡尔曼增益方程
    kfp['G'] = kfp['P'] / (kfp['P'] + kfp['R'])

    # 更新最优值方程
    kfp['Output'] = kfp['Output'] + kfp['G'] * (input - kfp['Output'])
      
    # 更新方差方程
    kfp['P'] = (1 - kfp['G']) * kfp['P']
      
    return kfp['Output']

def moving_average_filter(new_value, history, window_size):
    """移动平均滤波
    
    Args:
        new_value: 新采样值
        history: 历史数据列表
        window_size: 窗口大小
        
    Returns:
        滤波后的值
    """
    # 更新历史数据
    history[history_idx] = new_value
    
    # 计算平均值
    filtered_value = sum(history) / window_size
    
    return filtered_value

def exponential_moving_average(new_value, prev_value, alpha):
    """指数移动平均滤波
    
    Args:
        new_value: 新采样值
        prev_value: 前一个滤波值
        alpha: 平滑因子
        
    Returns:
        滤波后的值
    """
    return alpha * new_value + (1 - alpha) * prev_value

# 核心板上 C4 是 LED
#led1 = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=True)
# 选择学习板上的二号拨码开关作为退出选择开关
end_switch = Pin('C19', Pin.IN, pull=Pin.PULL_UP_47K, value=True)
end_state = end_switch.value()

# 初始化屏幕
cs = Pin('C5', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
cs.high()
cs.low()
rst = Pin('B9', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
dc = Pin('B8', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
blk = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
drv = LCD_Drv(SPI_INDEX=1, BAUDRATE=60000000, DC_PIN=dc, RST_PIN=rst, LCD_TYPE=LCD_Drv.LCD200_TYPE)
lcd = LCD(drv)
lcd.color(0xFFFF, 0x0000)
lcd.mode(0)
lcd.clear(0x0000)

# 初始化无线串口
wireless = WIRELESS_UART(460800)
data_flag = wireless.data_analysis()
data_wave = [0, 0, 0, 0, 0, 0, 0, 0]
flag_debug = False
# 初始化电机
motor_l = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C25_DIR_C27, 20000, duty=0, invert=False)
motor_r = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C24_DIR_C26, 20000, duty=0, invert=True)

# 初始化编码器
encoder_l = encoder("D0", "D1", True)
encoder_r = encoder("D2", "D3")


# todo 初始化CCD
ccd = TSL1401(1)  # 采集分频数为5
ccd.set_resolution(TSL1401.RES_12BIT)  # 设置12位分辨率
ccd_data1 = ccd.get(0)  # 获取CCD1数据（前置）
ccd_data2 = ccd.get(1)  # 获取CCD2数据（后置）

# 初始化陀螺仪
imu = IMU660RX()

# 初始化按钮
key = KEY_HANDLER(10)
key_data = key.get()
# 陀螺仪数据
acc_zero = [0.0, 0.0, 0.0]
gyro_zero = [0.0, 0.0, 0.0]
imu_data = imu.get()
last_pitch = 0.0
pitch = 0.0
yaw = 0.0
roll = 0.0
acc_angle = 0.0
gyro_angle = 0.0
alpha = 0.80
last_gyro = 0.0

# 编码器数据滤波
encoder_l_data = 0
encoder_r_data = 0
encoder_l_data_last = 0
encoder_r_data_last = 0

# 编码器平均值滤波
encoder_l_data_ave = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
encoder_r_data_ave = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
encoder_l_idx = 0
encoder_r_idx = 0

# PID参数
# 角速度环PID参数
kp_gyro = 3.5#-6.3#-5#-6.3          -6.3
ki_gyro = 0.0
kd_gyro = 0.33#-2#-2                 -2
err_gyro = 0.0
err_gyro_last = 0.0
err_gyro_sum = 0.0
err_gyro_sum_limit = 100.0
ki_gyro_flag = 1

# 角度环PID参数
zero_angle = 39.45  # 机械零点
kp_angle = 90#300#191#200#351#250#120            200
ki_angle = 0.0
kd_angle =43 #24#188#60#174#-60#-30                 60
err_angle = 0.0
err_angle_last = 0.0
err_angle_sum = 0.0
err_angle_sum_limit = 200

# 速度环PID参数
kp_speed =-2.32#-0.06#-0.4#-0.49 #-0.095#-0.09#-0.2#-0.5#-0.2#-0.26         -0.18
ki_speed = 0#-0.006#-0.01#-0.01#-0.008#-0.02#-0.02                   -0.02
kd_speed = 0.16#0.08#0.01# -0.025 #-0.02#-0.02              0.12
err_speed_sum = 0
err_speed_last = 0
set_speed =-50  # 目标速度

# 转向环PID参数
kp_turn = 0 #-70#-10#16                 85
kp2_turn = 0 #0.3
kd_turn = 0 #10#-7 
kd2_turn = 0#-0.25
err_turn = 0
err_turn_last = 0
turn_pid_out = 0
turn_err = 0
# 定时器标志位和计数器
timer_count = 0
gyro_pid_flag = False
angle_pid_flag = False
speed_pid_flag = False
ccd_get_flag = False
angle_get_flag = False
motor_set_flag = False
turn_pid_flag = False
angle_hefa = False

# PID输出
angle_pid_out = 0.0
gyro_pid_out = 0.0
speed_pid_out = 0.0

# 速度数据
speed_l = 0.0
speed_r = 0.0
speed_l_ori = 0
speed_r_ori = 0

# 电机占空比
motor_duty_sum = [0.0, 0.0, 0.0]
motor_duty_sum_idx = 0

# 初始化速度管理器
speed_manager = SpeedManager()

# 速度控制参数
TURN_SPEED_REDUCTION = 0.1  # 弯道减速系数
MAX_SPEED = -90  # 最大速度
MIN_SPEED = -60 # 最小速度
BASE_SPEED = -80 # 基础速度

def calculate_turn_speed(turn_error):
    """计算弯道减速后的目标速度
    
    Args:
        turn_error: 转向偏差
        
    Returns:
        减速后的目标速度
    """
    global TURN_SPEED_REDUCTION,MAX_SPEED,MIN_SPEED,BASE_SPEED
    # 计算转向偏差的绝对值
    abs_turn_error = abs(turn_error)
    
    # 根据转向偏差计算减速系数
    reduction_factor = 1.0 - (abs_turn_error * TURN_SPEED_REDUCTION)
    
    # 确保减速系数在合理范围内
    reduction_factor = max(0.3, min(1.0, reduction_factor))
    
    # 计算减速后的目标速度
    target_speed = BASE_SPEED * reduction_factor
    
    # 确保速度在允许范围内
    target_speed = max(MIN_SPEED, min(MAX_SPEED, target_speed))
    
    return target_speed

# 函数定义
def quaternion_update(imu_data):
    """使用四元数更新姿态角度"""
    global pitch, yaw, roll
    
#     # 获取加速度和角速度数据
#     acc_x = imu_data[0]
#     acc_y = imu_data[1]
#     acc_z = imu_data[2]
#     
#     gyro_x = imu_data[3]
    gyro_y = imu_data[4]
#     gyro_z = imu_data[5]
#     
#     # 转换为物理单位
#     acc_x = acc_x * 0.244 / 1000  # g
#     acc_y = acc_y * 0.244 / 1000  # g
#     acc_z = acc_z * 0.244 / 1000  # g
#     
#     gyro_x = gyro_x * 0.07 * 0.0174  # rad/s
    gyro_y = gyro_y * 0.07 * 0.0174  # rad/s
#     gyro_z = gyro_z * 0.07 * 0.0174  # rad/s
#     
    # 使用my_imu.py中的四元数解算函数
    
    imu_turn_data = turn_imu_data(imu_data)
    angles = angle_cal(imu_turn_data)
    pitch = angles[0]  # 俯仰角
    roll = angles[1]   # 横滚角
    yaw = angles[2]    # 偏航角
    pitch_ori =  angles[3]
    # 计算用于角速度控制的角速度值
    y_gyro = gyro_y / 0.0174  # 转换回deg/s
    
    return roll, y_gyro,yaw,pitch_ori

def encoder_get_filtered():
    """编码器数据滤波"""
    global encoder_l, encoder_r, speed_l, speed_r, speed_l_ori, speed_r_ori
    global speed_l_history, speed_r_history, history_idx, speed_l_ema, speed_r_ema
    
    # 获取原始编码器数据
    speed_l_ori = encoder_l.get()
    speed_r_ori = encoder_r.get()
    
    # 使用移动平均滤波
    speed_l = moving_average_filter(speed_l_ori, speed_l_history, MA_WINDOW_SIZE)
    speed_r = moving_average_filter(speed_r_ori, speed_r_history, MA_WINDOW_SIZE)
    
    # 使用指数移动平均滤波
    speed_l_ema = exponential_moving_average(speed_l_ori, speed_l_ema, EMA_ALPHA)
    speed_r_ema = exponential_moving_average(speed_r_ori, speed_r_ema, EMA_ALPHA)
    
    # 更新历史索引
    history_idx = (history_idx + 1) % MA_WINDOW_SIZE
    
    # 发送数据到示波器（可选）
    #wireless.send_oscilloscope(speed_l, speed_r, speed_l_ori, speed_r_ori)
    
    # 返回滤波后的值（这里使用移动平均的结果，你也可以选择使用指数移动平均的结果）
    return speed_l, speed_r

def my_queue_add():
    """编码器数据队列添加（平均值滤波）"""
    global encoder_l_dannual_out_offsetata_ave, encoder_r_data_ave, encoder_l_idx, encoder_r_idx, encoder_l, encoder_r
    
    encoder_l_data_ave[encoder_l_idx] = encoder_l.get()
    encoder_r_data_ave[encoder_r_idx] = encoder_r.get()
    
    encoder_l_idx += 1
    encoder_r_idx += 1
    
    encoder_l_idx %= 10
    encoder_r_idx %= 10
    return encoder_l_data_ave, encoder_r_data_ave

def encoder_get_avefliter_init():
    """编码器平均值滤波初始化"""
    for i in range(0, 10):
        my_queue_add()
        
def encoder_get_avefliter():
    """编码器平均值滤波"""
    speed_l_queue, speed_r_queue = my_queue_add()
    
    res_l = sum(speed_l_queue)
    res_r = sum(speed_r_queue)
    return (res_l / 10), (res_r / 10)

#todo 前馈控制改进版角速度环
K_ford_g = 0

def gyro_pid(target, y_gyro):
    """角速度环PID控制"""
    global kp_gyro, kd_gyro, err_gyro, err_gyro_last, err_gyro_sum, ki_gyro_flag, err_gyro_sum_limit,K_ford_g

    err_gyro = target - y_gyro
    err_gyro_sum += err_gyro
    
    out_ford = target * K_ford_g
    if(err_gyro_sum > err_gyro_sum_limit):
        err_gyro_sum = err_gyro_sum_limit 
    elif(err_gyro_sum < -err_gyro_sum_limit):
        err_gyro_sum = -err_gyro_sum_limit

    out = kp_gyro * err_gyro + kd_gyro * (err_gyro - err_gyro_last)
    err_gyro_last = err_gyro
#     wireless.send_oscilloscope(out_ford,out)
    return out + out_ford
K_ford_a = 0
def angle_pid(target_angle, now_angle):
    """角度环PID控制"""
    global kp_angle, ki_angle, kd_angle, err_angle, err_angle_last, err_angle_sum, err_angle_sum_limit,K_ford_a
    
    err_angle_last = err_angle
    err_angle = target_angle - now_angle
    err_angle_sum += err_angle
    
    out_ford = K_ford_a * (target_angle - 40.45)
    if(err_angle_sum > err_angle_sum_limit):
        err_angle_sum = err_angle_sum_limit
    if(err_angle_sum < -err_angle_sum_limit):
        err_angle_sum = -err_angle_sum_limit
   
    out = kp_angle * err_angle + ki_angle * err_angle_sum + kd_angle * (err_angle - err_angle_last)
#     wireless.send_oscilloscope(out_ford,out,now_angle)
    return out + out_ford
count_i_speed = 0
def speed_pid(target, now_speed):
    """速度环PID控制"""


    global kp_speed, ki_speed, kd_speed, err_speed_sum, err_speed_last, count_i_speed
    count_i_speed += 50
    err_speed = target - now_speed
    err_speed_sum += err_speed
    #积分限幅
    err_speed_sum = min(200,max(-200,err_speed_sum))
     
    p = kp_speed * err_speed
    if(count_i_speed > 8000):
        i = ki_speed * err_speed_sum
    else:
        i = 0
    d = kd_speed * (err_speed - err_speed_last)
    
    out = p + i + d
    
    
    
    err_speed_last = err_speed
    if(out > 15):
        out = 15
    elif(out < -15):
        out = -15
     
    return out

def turn_pid(err):
    """转向环PID控制"""
    global kp_turn, kp2_turn, kd_turn, kd2_turn, err_turn, turn_pid_out, err_turn_last,imu_data,wireless
    
    err_turn_last = err_turn
    err_turn = err
    
    turn_pid_out = kp_turn * err_turn + kd_turn * (err_turn - err_turn_last) + kp2_turn * (math.fabs(err_turn) * err_turn)  + kd2_turn * imu_data[5]
#     wireless.send_oscilloscope(kp_turn * err_turn,kd_turn * (err_turn - err_turn_last),err)
    return turn_pid_out

timer2_count = 0
def time_pit_handler2(time):
    global key_data,lcd,ccd_data1,ccd_data2
    global flag_debug,timer2_count
#     show_menu(lcd,ccd_data1,ccd_data2)
#     lcd.wave(0, 0, 128, 64, ccd_data1, max = 4095)
#     lcd.wave(0, 64, 128, 64, ccd_data2, max = 4095)
#     lcd.wave(0, 128, 128, 64, my_ccd.ccd1_image, max = 4095)
#     lcd.wave(0, 192, 128, 64, my_ccd.ccd2_image, max = 4095)
#     timer2_count += 10
#     
    if(timer2_count %200 == 0):
        flag_debug = True 
    if key_data[0]:
        key_handler(0,lcd)
        key.clear(1)
    if key_data[1]:
        key_handler(1,lcd)
        key.clear(2)
    if key_data[2]:
        key_handler(2,lcd)
        key.clear(3)
    if key_data[3]:
        key_handler(3,lcd)
        key.clear(4)
    
        
    
    
    
def time_pit_handler1(time):
    """定时器1回调函数"""
    global gyro_pid_flag, angle_pid_flag, speed_pid_flag, angle_get_flag, ccd_get_flag
    global motor_set_flag, turn_pid_flag, timer_count, data_flag, zero_angle
    global motor_l, motor_r, pitch, encoder_l, encoder_r, motor_duty_max, lcd, angle_hefa, set_speed
    
    timer_count += 1
    #print(timer_count)
    # 前一段时间先让四元数收敛
    if timer_count > 1000:
        angle_hefa = True
    
    
    
    # 标志位设置（控制执行频率）
    if timer_count %2 == 0:
        gyro_pid_flag = True
        
        if(timer_count < 1000):
            motor_r.duty(0)
            motor_l.duty(0)
        else:
            motor_set_flag = True
    if timer_count % 5 == 0:
        angle_pid_flag = True
        angle_get_flag = True
        
    if timer_count % 10 == 0:
        
        turn_pid_flag = True
        ccd_get_flag = True
    if timer_count % 20 == 0:
        speed_pid_flag = True
       
    # 角度过大保护
    
#     if abs(pitch - zero_angle) > 20:
#         motor_r.duty(0)
#         motor_l.duty(0)
#         motor_set_flag = False
#todo 简陋的十字处理
duty_l_last = 0
duty_r_last = 0
cross_flag  =0
motor_duty_left_offset = 900
motor_duty_right_offset = 800
#todo 简陋的调参
def debug_update_pid():
    global data_flag,wireless,kp_angle,kd_angle,data_wave,kp_speed,ki_speed,kd_speed,kp_gyro,kd_gyro,motor_duty_left_offset,motor_duty_right_offset
    global K_ford_g,K_ford_a
#     a = utime.ticks_us()
    data_flag = wireless.data_analysis()
   
    for i in range(0,8):
        if data_flag[i]:
            data_wave[i] = wireless.get_data(i)
    kp_angle = data_wave[0]
    kd_angle = data_wave[1]        
    kp_speed = data_wave[2]  
    ki_speed = data_wave[3]  
    kd_speed = data_wave[4]
    kp_gyro =data_wave[5]
    motor_duty_left_offset = data_wave[6]
    motor_duty_right_offset = data_wave[7]
#     b = utime.ticks_us()
#     print(f'{utime.ticks_diff(b,a)}')
    
    
distance_T = 0        
baohu_jishi = 0
#todo 主控制函数
def process_control():
    """主控制处理函数"""
    global gyro_pid_flag, angle_pid_flag, speed_pid_flag, ccd_get_flag, angle_get_flag, turn_pid_flag
    global motor_set_flag, timer_count, imu, imu_data, pitch, roll, yaw, last_pitch, zero_angle
    global angle_pid_out, gyro_pid_out, speed_pid_out, turn_pid_out, led1, ccd_data1, ccd_data2
    global speed_l, speed_r, speed_l_ori, speed_r_ori, motor_duty_sum, motor_duty_sum_idx
    global angle_hefa, encoder_l, encoder_r, motor_l, motor_r, set_speed,turn_err,distance_T
    global flag_debug,data_flag,wireless
    global duty_l_last, duty_r_last,cross_flag,baohu_jishi,motor_duty_left_offset,motor_duty_right_offset
    # 角度获取处理
    if angle_get_flag:
        angle_get_flag = False
        pitch, y_gyro ,yaw,pitch_ori= quaternion_update(imu_data)
#         pitch,roll,yaw,y_gyro,ac_p = get_kalman_imu(imu_data)
#         kalman_angles, accel_angle = kalman_angle_cal(imu_data)
#         pitch,roll,yaw= kalman_angles
#         ac_p = accel_angle[0]
        
        
        
#         wireless.send_oscilloscope(pitch,ac_p,roll,yaw,imu_data[3])
#         wireless.send_oscilloscope(pitch,pitch_ori)
    
#     #调试
    if flag_debug:
        debug_update_pid()
        flag_debug = False 
    if ccd_get_flag:
#         a = utime.ticks_us()
        ccd_get_flag = False
#         a = utime.ticks_ms()
        turn_err,cross_flag = my_ccd.get_ccd_err(ccd_data1,ccd_data2,imu_data,speed_l,speed_r)
#         
#         lcd.str16(0, 128+16,"n:{:d}".format(my_ccd.n),0xF800)
#         lcd.str16(0, 128+32,"N:{:d}".format(my_ccd.N),0xF800)
#         lcd.str16(0, 128+48,"flag:{:d}".format(my_ccd.flag),0xF800)
#         lcd.wave(0,  0, 128, 64, ccd_data1, max = 4095)
#         lcd.wave(0, 64, 128, 64, ccd_data2, max = 4095)
#         lcd.str16(0, 128+112,"ccd1_r:{:d}".format(my_ccd.ccd1_right_idx),0xF800)
#         lcd.str16(0, 128+64,"ccd1_l:{:d}".format(my_ccd.ccd1_left_idx),0xF800)
#         lcd.str16(0, 128+128,"ccd2_l:{:d}".format(my_ccd.ccd2_left_idx),0xF800)
#         
#         lcd.str16(0, 128+96,"angle:{:f}".format(my_ccd.gyro_z1),0xF800)

#         wireless.send_oscilloscope(my_ccd.ccd1_left_idx)
#         lcd.str16(0, 128+32,"ccd1_l:{:0>3d}".format(my_ccd.ccd1_left_idx),0xF800)
#         lcd.str16(0, 128+48,"ccd1_r:{:0>3d}".format(my_ccd.ccd1_right_idx),0xF800)
#         lcd.str16(0, 128+64,"ccd2_l:{:0>3d}".format(my_ccd.ccd2_left_idx),0xF800)
#         lcd.str16(0, 128+80,"ccd2_r:{:0>3d}".format(my_ccd.ccd2_right_idx),0xF800)
#         lcd.str16(0, 128+96,"ccd2_cen:{:0>3d}".format(my_ccd.ccd2_center),0xF800)
        
#         lcd.str16(0, 128+32,"ccd1_l:{:0>3d}".format(my_ccd.left_point_1),0xF800)
#         lcd.str16(0, 128+48,"ccd1_r:{:0>3d}".format(my_ccd.right_point_1),0xF800)
#         lcd.str16(0, 128+64,"ccd2_l:{:0>3d}".format(my_ccd.left_point_2),0xF800)
#         lcd.str16(0, 128+80,"ccd2_r:{:0>3d}".format(my_ccd.right_point_2),0xF800)
#         lcd.str16(0, 128+96,"ccd2_cen:{:0>3d}".format(my_ccd.ccd2_center),0xF800)
#         lcd.str16(0, 128+16,"ccd2:{:0>3d}".format(my_ccd.ac_ccd2_road_len),0xF800)
        # 根据转向偏差计算目标速度
#         wireless.send_oscilloscope(my_ccd.distance_T,my_ccd.m,my_ccd.gyro_z1)

#         wireless.send_oscilloscope(my_ccd.x, my_ccd.left_point_2, my_ccd.right_point_2, my_ccd.left_point_1, my_ccd.right_point_1, my_ccd.mid_point_2, my_ccd.ccd2_left_idx_buxian, my_ccd.ccd2_right_idx_buxian)
#         b = utime.ticks_us()
#         print(f'{utime.ticks_diff(b,a)}')
#         wireless.send_oscilloscope(my_ccd.x, my_ccd.ccd2_left_idx, my_ccd.ccd2_right_idx, my_ccd.ccd1_left_idx, my_ccd.ccd1_right_idx, my_ccd.ccd2_center, my_ccd.ccd2_left_idx_buxian, my_ccd.ccd2_right_idx_buxian)
#         wireless.send_oscilloscope(my_ccd.ccd1_left_idx,my_ccd.ccd1_right_idx,my_ccd.ccd2_left_idx,my_ccd.ccd2_right_idx,my_ccd.n,my_ccd.N,my_ccd.flag)
        set_speed = calculate_turn_speed(turn_err)
       
        
#         wireless.send_oscilloscope(turn_err)
    # 只有在angle_hefa为真时，才进行PID控制和电机控制
    if angle_hefa:
        # 角度环PID控制
        if angle_pid_flag:
            
            angle_pid_flag = False
            angle_pid_out = angle_pid(zero_angle + speed_pid_out, pitch)
            
#             print(f'{utime.ticks_diff(b,a)}')
          
        # 角速度环PID控制
        if gyro_pid_flag:
#             a = utime.ticks_us()
            gyro_pid_flag = False
            gyro_pid_out = gyro_pid(angle_pid_out, imu_data[3])
#             b = utime.ticks_us()
#             wireless.send_str(f'{utime.ticks_diff(b,a)}\n') 
#             print(gyro_pid_out)
        # 速度环PID控制
#         speed_current = 0
        if speed_pid_flag:
            speed_pid_flag = False
            speed_l_ori = encoder_l.get()
            speed_r_ori = encoder_r.get()
            speed_l = kalman_filter(kfp_var_l,speed_l_ori)
            speed_r = kalman_filter(kfp_var_r,speed_r_ori)
            speed_current = (speed_l + speed_r) / 2
#             distance_T += speed_current / 1024 * 30 / 50 * 0.05 * 3.1415926
#             wireless.send_oscilloscope(speed_l,speed_l_ori,speed_r_ori,speed_r)
            speed_pid_out = speed_pid(0, speed_current)
#             wireless.send_oscilloscope(distance_T)
#             speed_pid_out = speed_pid(0, speed_current)
#             print(speed_pid_out)
            #print(speed_current)
        # 转向环PID控制
        if turn_pid_flag:
            turn_pid_flag = False 
            turn_pid_out = turn_pid(turn_err)
        
        # 电机控制输出
        if motor_set_flag:
            
#             lcd.str16(0, 0,"{:d}".format(cross_flag),0xF800)
#             lcd.str16(0, 16,"{:d}".format(duty_l_last),0xF800)
        
            
            # 合并各环节输出计算最终电机占空比
            motor_duty_left = gyro_pid_out #- turn_pid_out
            motor_duty_right = gyro_pid_out #+ turn_pid_out
            
#             duty_l_last = motor_duty_left
#             duty_r_last = motor_duty_right
#             wireless.send_oscilloscope(turn_pid_out,motor_duty_left)
            # 限制电机占空比
            if motor_duty_left > 7000:
                motor_duty_left = 7000
            elif motor_duty_left < -7000:
                motor_duty_left = -7000
                
            if motor_duty_right > 7000:
                motor_duty_right = 7000
            elif motor_duty_right < -7000:
                motor_duty_right = -7000
            
            if motor_duty_left > 0:
                motor_duty_left += motor_duty_left_offset
            else:
                motor_duty_left -= motor_duty_left_offset
            
            if motor_duty_right > 0:
                motor_duty_right += motor_duty_right_offset
            else:
                motor_duty_right -= motor_duty_right_offset
            
            
            
            motor_l.duty(int(motor_duty_left))
            motor_r.duty(int(motor_duty_right))
#             motor_l.duty(3000)
#             motor_r.duty(3000)
#             if cross_flag == 1:
#                 motor_l.duty(int(duty_l_last))
#                 motor_r.duty(int(duty_r_last))
#                 
#             else:
#                 motor_l.duty(int(motor_duty_left))
#                 motor_r.duty(int(motor_duty_right))
                
                

#             
#             wireless.send_oscilloscope(cross_flag)   
#             motor_l.duty(int(2000))
#             motor_r.duty(int(2000))
#             
        
            
def menu_start():
    global lcd,ccd_data1,ccd_data2,kp_gyro,kd_gyro,kp_angle,kd_angle,kp_speed,kd_speed,pitch
    '''
    从菜单开始运行

    '''
    while getshowflag():
        
        show_menu(lcd,ccd_data1,ccd_data2)
      
    kp_gyro,kd_gyro,kp_angle,kd_angle,kp_speed,kd_speed = getdebugmenu()
    
    
def main():
    """主函数"""
    global end_switch, end_state, lcd, timer_count, angle_hefa,imu, imu_data, ccd,key,key_data,pit1,pit2,pit3
    
   
    
    
    # 初始化
    encoder_get_avefliter_init()
    
    # 设置速度控制模式
#     speed_manager.set_mode(0)  # 普通模式
    
    # 初始化定时器
    pit1 = ticker(1)
    pit2 = ticker(0)
    pit3 =  ticker(2)
    pit2.capture_list(key,ccd)
    pit1.capture_list(imu)
    pit3.capture_list(encoder_l,encoder_r)
    pit1.callback(time_pit_handler1)
    pit2.callback(time_pit_handler2)
    pit1.start(1)  # 5ms周期
    pit2.start(10)
    pit3.start(10)
    imu_data = imu.get()
    key_data = key.get()
    


#     menu_start()
    
        # 主循环
    while True:
        # 执行控制处理
#         a = utime.ticks_us()
        process_control()
#         b = utime.ticks_us()
        
#         print(f"图像运行时间：{utime.ticks_diff(b,a)}us")   
            # 检查停止条件
        if end_switch.value() != end_state:
                # 停止电机
            motor_l.duty(0)
            motor_r.duty(0)
                # 停止定时器
            #print(4444)
            pit1.stop()
            pit2.stop()
                
                # 显示停止信息
               
            break
            
            # 回收内存
        gc.collect()

# 程序入口
if __name__ == "__main__":
    main() 


